/**
 *@note Copyright 2012-2022 CETHIK All Rights Reserved.
 *@brief 障碍层文件
 *@author tuyinan
 *@data 2022-04-xx
 */
#ifndef COSTMAP_2D_OBSTACLE_LAYER_H_
#define COSTMAP_2D_OBSTACLE_LAYER_H_

#include <costmap_2d/costmap_layer.h>
#include <costmap_2d/layered_costmap.h>

// 临时的获取数据的接口，后期可以替代为其他文件
#include <costmap_2d/data_collector.h>
#include <costmap_2d/footprint.h>
#include <costmap_2d/observation_buffer.h>

#include <thread>

namespace costmap_2d {
/**
 *@brief 障碍层
 *@author tuyinan
 *@version 2022-04-xx
 */
class ObstacleLayer : public CostmapLayer {
 public:
  /**
   *@brief 构造函数
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  ObstacleLayer() {
    costmap_ =
        NULL;  // this is the unsigned char* member of parent class Costmap2D.
  }
  /**
   *@brief 析构函数
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual ~ObstacleLayer();
  /**
   *@brief 初始化函数
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual void onInitialize();
  /**
   *@brief 获取点云的线程
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual void getDatafromCollector();
  /**
   *@brief 获取位姿的线程
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual void getOdomfromCollector();
  /**
   *@brief 更新地图边际
   *@param double robot_x 机器人位姿x
   *@param double robot_y 机器人位姿y
   *@param double robot_yaw 机器人角度
   *@param double* min_x 最小x
   *@param double* min_y 最小y
   *@param double* max_x 最大x
   *@param double* max_y 最大y
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual void updateBounds(double robot_x, double robot_y, double robot_yaw,
                            double* min_x, double* min_y, double* max_x,
                            double* max_y);
  /**
   *@brief 更新代价
   *@param costmap_2d::Costmap2D& 代价地图类
   *@param int min_i 最小的x
   *@param int min_j 最小的y
   *@param int max_i 最大的x
   *@param int max_j 最大的y
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
                           int min_j, int max_i, int max_j);

  /**
   *@brief 获取用来标记障碍的观测类
   *@param  std::vector<costmap_2d::Observation>& marking_observations
   *标记障碍的观测类
   *@return bool 成功或失败
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  bool getMarkingObservations(
      std::vector<costmap_2d::Observation>& marking_observations) const;

  /**
   *@brief 获取用来清除的观测类
   *@param  std::vector<costmap_2d::Observation>& clearing_observations
   *用来清除的观测类
   *@return bool 成功或失败
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  bool getClearingObservations(
      std::vector<costmap_2d::Observation>& clearing_observations) const;
  /**
   *@brief 通过清除观测类清除地图上的障碍点
   *@param double* min_x 最小x
   *@param double* min_y 最小y
   *@param double* max_x 最大x
   *@param double* max_y 最大y
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual void raytraceFreespace(
      const costmap_2d::Observation& clearing_observation, double* min_x,
      double* min_y, double* max_x, double* max_y);
  /**
   *@brief 激活层
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual void activate();
  /**
   *@brief 取消激活
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual void deactivate();
  /**
   *@brief 重置
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual void reset();

  /**
   *@brief 更新激光边界信息
   *@param double ox 第一个点的x
   *@param double oy 第一个点的y
   *@param double wx 第二个点的x
   *@param double wy 第二个点的y
   *@param double range 范围
   *@param double* min_x 最小x
   *@param double* min_y 最小y
   *@param double* max_x 最大x
   *@param double* max_y 最大y
   *@return
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void updateRaytraceBounds(double ox, double oy, double wx, double wy,
                            double range, double* min_x, double* min_y,
                            double* max_x, double* max_y);
  /// 转换后的占位
  std::vector<costmap_2d::Point> transformed_footprint_;
  /// 是否清除机器人本身占位
  bool footprint_clearing_enabled_;
  /**
   *@brief 更新占位
   *@param double robot_x 机器人位姿x
   *@param double robot_y 机器人位姿y
   *@param double robot_yaw 机器人角度
   *@param double* min_x 最小x
   *@param double* min_y 最小y
   *@param double* max_x 最大x
   *@param double* max_y 最大y
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void updateFootprint(double robot_x, double robot_y, double robot_yaw,
                       double* min_x, double* min_y, double* max_x,
                       double* max_y);
  /// 障碍物最大高度
  double max_obstacle_height_;
  /// 数据处理类
  boost::shared_ptr<costmap_2d::ObservationBuffer> observation_buffers_;
  /// 标记障碍类
  boost::shared_ptr<costmap_2d::ObservationBuffer>
      marking_buffers_; 
  /// 清除障碍类
  boost::shared_ptr<costmap_2d::ObservationBuffer>
      clearing_buffers_;  ///< @brief Used to store observation buffers used for
                          ///< clearing obstacles
  /// 是否采用移动窗口
  bool rolling_window_;
  /// 地图合并方法
  int combination_method_;
  /// 机器人到地图坐标转换矩阵
  Eigen::Transform<float, 3, Eigen::Isometry> odomTransformEigen_;
  /// 机器人所在位置
  costmap_2d::Point origin_;
  /// 是否获取数据
  bool collect_signal_;
};

}  // namespace costmap_2d

#endif  // COSTMAP_2D_OBSTACLE_LAYER_H_

